#include "servo.h"
#include "uart.h"
#include "basic.h"
#define PWM_PERIOD_COUNT (1000)
void servo_init(void)
{
    //DL_Timer_startCounter(SERVO_PWM_INST);
}
void servo_set_angle(uint8_t angle)
{
    // T = 20ms = 20000us ~PWM_PERIOD_COUNT
    int duty = (angle * 2000 / 180) + 500;
    int period_cnt = PWM_PERIOD_COUNT - duty * PWM_PERIOD_COUNT / 20000;
   // DL_TimerG_setCaptureCompareValue(SERVO_PWM_INST, period_cnt, DL_TIMER_CC_1_INDEX);
}

void bus_servo_set_pwm(uint8_t servo_idx, uint16_t pwm, uint16_t time)
{
    if (pwm > 2500)
        pwm = 2500;

    char str[30];
    sprintf(str, "#%03dP%04dT%04d!", servo_idx, pwm, time);
    uart1_send_str(str);
}
void bus_servo_change_ID(uint8_t O_servo_idx, uint8_t N_servo_idx)
{
    char str[30];

    sprintf(str, "#%03dPID%03d!", O_servo_idx, N_servo_idx);

    uart1_send_str(str);
}
void bus_servo_set_mode(uint8_t servo_idx, bus_servo_mode_t servo_MODE)
{
    char str[30];
    sprintf(str, "#%03dPMOD%d!", servo_idx, (int)servo_MODE);
    uart1_send_str(str);
}
void bus_servo_set_id(uint8_t servo_idx_target)
{
    bus_servo_change_ID(255, servo_idx_target);
}
